/**
* \author You!
*
* \file  Algebra3D.h
*
* \brief Linear Algebra package including Point3, Vector3, and Matrix4.
*        All the member functions below are inlined for speed.
*/

#ifndef ALGEBRA3D_H
#define ALGEBRA3D_H

#include <math.h>
#include <stdio.h>
#include <iostream>
using namespace std;


/** Note, the code is setup to be able to switch between using "float" and
* "double" as the base type for storing x,y,z data in the Point3, Vector3
* and Matrix4 classes.  Hill & Kelley use float in their example code, but
* for many applications you'll want the extra precision provided by doubles.
* stick with doubles for the algebra assignment to be consistent with the
* tester and camera code.
*/
//#define AReal float
#define AReal double


/** The == and != opeartors have to account for some numberical imprecision,
* so rather than testing to see whether two numbers are **exactly** the same
* you'll want to check to see whether they are the same plus or minus EPSILON. 
*/
#define EPSILON 1e-8




/** @class Point3 
* @brief 3D Point with floating point coordinates.
*/
class Point3 
{  
public:  
	//! Default point at the origin
	Point3() {x = y = z = 0;} 

	//! Constructs a point given (x,y,z, 1)
	Point3(AReal xx, AReal yy, AReal zz) {x = xx; y = yy; z = zz;} 

	//! Constructs a point given a pointer to x,y,z data
	Point3(AReal *p) { x = p[0]; y = p[1]; z = p[2]; }

	//! Copy constructor for point
	Point3(const Point3& p) { x = p.x; y = p.y; z = p.z; }

	//! Point destructor (does nothing)
	virtual ~Point3() {}

	//! Check for "equality" -- take floating point imprecision into account
	inline bool operator==(const Point3& p) const {
		return p.x >= x-EPSILON && p.x <= x+EPSILON && 
			p.y >= y-EPSILON && p.y <= y+EPSILON &&
			p.z >= z-EPSILON && p.z <= z+EPSILON;
	}

	//! Check for "inequality" -- take floating point imprecision into account
	inline bool operator!=(const Point3& p) const {
		return p.x < x-EPSILON && p.x > x+EPSILON && 
			p.y < y-EPSILON && p.y > y+EPSILON &&
			p.z < z-EPSILON && p.z > z+EPSILON;
	}

	//! Assignment operator
	inline Point3& operator=(const Point3& p) {
		x = p.x; y = p.y; z = p.z;
		return *this;		
	}

	//! Accesses the ith coordinate of the point
	inline AReal operator[](const int i) const { 
		if (i==0) return x;
		else if (i==1) return y;
		else if (i==2) return z;
		else if (i==3) return 1.0; // w component of a point is 1 so return the constant 1.0
		else {
			printf("Point3 index out of bounds for [] operator.\n");
			return 1.0;
		}
	}

	//! Accesses the ith coordinate of the point
	inline AReal& operator[](const int i) { 
		if (i==0) return x;
		else if (i==1) return y;
		else if (i==2) return z;
		else {
			// Note: can't return a reference to 1.0 for (i==3)
			printf("Point3 index out of bounds for [] operator.\n");
			return x;
		}
	}

	//! Prints out the point (for debugging)
	inline void print() const { printf("(%g,%g,%g)\n",x,y,z); }

	//! "Unpacks" the data, putting it into a standard C array with 4 elements to account
	//! for homogeneous coordinates.
	inline void unpack(AReal *p) const {
		p[0]=x; p[1]=y; p[2]=z; p[3]=1.0;
	}

public:
	AReal x,y,z; 
};




/** @class Vector3 
* @brief 3D vector containing both magnitude and direction.
*/
class Vector3 
{  
public:  
	//! Default constructor to create zero vector
	Vector3() { x = y = z = 0; }

	//! Constructs a vector (x, y, z, 0)
	Vector3(AReal xx, AReal yy, AReal zz) {x = xx; y = yy; z = zz;} 

	//! Constructs a vector given a pointer to x,y,z data
	Vector3(AReal *v) { x = v[0]; y = v[1]; z = v[2]; }

	//! Copy constructor for vector
	Vector3(const Vector3& v) {x = v.x; y = v.y; z = v.z;} 

	//! Vector destructor (does nothing)
	virtual ~Vector3() {}

	//! Check for "equality" -- take floating point imprecision into account
	inline bool operator==(const Vector3& v) const {
		return v.x >= x-EPSILON && v.x <= x+EPSILON && 
			v.y >= y-EPSILON && v.y <= y+EPSILON &&
			v.z >= z-EPSILON && v.z <= z+EPSILON;
	}

	//! Check for "inequality" -- take floating point imprecision into account
	inline bool operator!=(const Vector3& v) const {
		return v.x < x-EPSILON && v.x > x+EPSILON && 
			v.y < y-EPSILON && v.y > y+EPSILON &&
			v.z < z-EPSILON && v.z > z+EPSILON;
	}

	//! Vector assignment operator
	inline Vector3& operator=(const Vector3& v) {
		x = v.x; y = v.y; z = v.z;
		return *this;		
	}

	//! Returns the ith coordinate of the vector
	inline AReal operator[](const int i) const { 
		if (i==0) return x;
		else if (i==1) return y;
		else if (i==2) return z;
		else if (i==3) return 0.0; // w component for a vector is 0.0, so return the const 0.0
		else {
			printf("Vector3 index out of bounds for [] operator.\n");
			return 0.0;
		}
	}

	//! Returns the ith coordinate of the vector
	inline AReal& operator[](const int i) { 
		if (i==0) return x;
		else if (i==1) return y;
		else if (i==2) return z;
		else {
			// Note print error even in the i==3 case here because can't return a reference to 0.0
			printf("Vector3 index out of bounds for [] operator.\n");
			return x;
		}
	}

	//! Prints the vector to standard out (for debugging)
	inline void print() const { printf("<%g,%g,%g>\n",x,y,z); }

	//! "Unpacks" the data, putting it into a standard C array with 4 elements to account
	//! for homogeneous coordinates.
	inline void unpack(AReal *v) const {
		v[0]=x; v[1]=y; v[2]=z; v[3]=0.0;
	}

public:
	AReal x,y,z; 
};



/** @class Matrix4
* @brief A 4x4 transformation matrix
*/
class Matrix4 
{
public: 
	//! Default constructor creates an identity matrix:
	Matrix4() {
		m[0] = m[5]  = m[10] = m[15] = 1.0; 
		m[1] = m[2]  = m[3]  = m[4]  = 0.0; 
		m[6] = m[7]  = m[8]  = m[9]  = 0.0; 
		m[11]= m[12] = m[13] = m[14] = 0.0; 
	} 

	//! Constructs a matrix given the elments.  Be careful, the elements are 
	//! passed in using row major order, so the matrix looks "correct" on the
	//! screen if you write this constructor using 4 lines as it is below.  But,
	//! to stay consistent with OpenGL, the matrix is stored internally in
	//! column major order!
	Matrix4(const AReal a, const AReal b, const AReal c, const AReal d, 
		const AReal e, const AReal f, const AReal g, const AReal h, 
		const AReal i, const AReal j, const AReal k, const AReal l, 
		const AReal mm, const AReal n, const AReal o, const AReal p) {
			m[0]=a; m[4]=b; m[8]=c; m[12]=d;
			m[1]=e; m[5]=f; m[9]=g; m[13]=h;
			m[2]=i; m[6]=j; m[10]=k; m[14]=l;
			m[3]=mm; m[7]=n; m[11]=o; m[15]=p;
	}

	//! Constructs a matrix given a 16 element array
	Matrix4(const AReal* a) { memcpy(m,a,16*sizeof(AReal)); }

	//! Copy constructor
	Matrix4(const Matrix4& m2) { memcpy(m,m2.m,16*sizeof(AReal)); }

	//! Destructor (does nothing)
	virtual ~Matrix4() {}

	//! Check for "equality" -- take floating point imprecision into account
	inline bool operator==(const Matrix4& m2) const {
		for(int a = 0; a < 16; a++)
		{
			if (m2.m[a] < m[a]-EPSILON || m2.m[a] > m[a]+EPSILON)
				return false;
		}
		return true;
	}  

	//! Check for "inequality" -- take floating point imprecision into account
	inline bool operator!=(const Matrix4& m2) const {
		for(int a = 0; a < 16; a++)
		{
			if (m2.m[a] < m[a]-EPSILON || m2.m[a] > m[a]+EPSILON)
				return true;
		}
		return false;
	}

	//! Matrix assignment operator
	inline Matrix4& operator=(const Matrix4& m2) {
		memcpy(m,m2.m,16*sizeof(AReal));
		return *this;
	}

	//! Returns the (r,c) elment of the matrix
	inline AReal operator()(const int r, const int c) const { return m[c*4+r]; }

	//! Returns the (r,c) elment of the matrix
	inline AReal& operator()(const int r, const int c) { return m[c*4+r]; }

	//! Prints the matrix to standard out
	inline void print(void) {
		for (int r=0; r<4; r++) {
			for (int c=0; c<4; c++)
				cout << m[c*4 + r] << " ";
			cout << endl;
		}
	} 

	//! "Unpacks" the data, putting it into a standard C array with 16 elements
	inline void unpack(AReal *m2) const {
		memcpy(m2, m, 16*sizeof(AReal));
	}

public:		
	//! holds a 4 by 4 matrix in column major order
	AReal m[16]; 
};





// ---------- Global Operators ---------- 



// --- Scalers ---

//! Scale a vector by 1/s, returns scaled vector
inline Vector3 operator/(const Vector3& v, const AReal s) {
	if(s == 0)
	{
		return Vector3(v.x/EPSILON, v.y/EPSILON, v.z/EPSILON);
	}else{
		return Vector3(v.x/s, v.y/s, v.z/s);
	}
}

//! Scale a vector by s, returns scaled vector
inline Vector3 operator*(const AReal s, const Vector3& v) {
	return Vector3(v.x*s, v.y*s, v.z*s);
}

//! Scale a vector by s, returns scaled vector (same as above, different order)
inline Vector3 operator*(const Vector3& v, const AReal s) {
	return Vector3(v.x*s, v.y*s, v.z*s);
}

//! Returns the negated vector
inline Vector3 operator-(const Vector3& v) {
	return Vector3(-v.x, -v.y, -v.z);
}

// Note: no -(point) operator



// --- Points and Vectors ---

//! Adds a vector and a point, returns a point
inline Point3 operator+(const Vector3& v, const Point3& p) {
	return Point3(v.x + p.x, v.y+p.y, v.z+p.z);
}

//! Adds a point and a vector, returns a point (same as previous, just different order)
inline Point3 operator+(const Point3& p, const Vector3& v) {
	return Point3(v.x + p.x, v.y+p.y, v.z+p.z);
}

//! Adds a vector and a vector, returns a vector
inline Vector3 operator+(const Vector3& v1, const Vector3& v2) {
	return Vector3(v1.x + v2.x, v1.y+v2.y, v1.z+v2.z);
}

// Note: no (point + point) operator

//! Subtracts a vector from a point, returns a point
inline Point3 operator-(const Point3& p, const Vector3& v) {
	return Point3(p.x-v.x, p.y-v.y, p.z-v.z);
}

//! Subtracts v2 from v1, returns a vector
inline Vector3 operator-(const Vector3& v1, const Vector3& v2) {
	return Vector3(v1.x - v2.x, v1.y-v2.y, v1.z-v2.z);
}

//! The vector spanning p1 and p2, make sure you know which direction it goes
inline Vector3 operator-(const Point3& p1, const Point3& p2) {
	// TODO
	return Vector3(p1.x-p2.x,p1.y-p2.y,p1.z-p2.z);
}

// Note: no (vector - point) operator



// --- Vector operations ---

//! Returns dot product of 2 vectors
inline AReal dot(const Vector3& u, const Vector3& v) {
	return u.x*v.x+u.y*v.y+u.z*v.z;
}

//! Returns cross product of 2 vectors
inline Vector3 cross(const Vector3& u, const Vector3& v) {
	return Vector3(u.y*v.z-u.z*v.y, u.z*v.x-u.x*v.z, u.x*v.y-u.y*v.x );
}

//! Returns length of a vector
inline AReal length(const Vector3& v) {
	return sqrt(v.x*v.x+v.y*v.y+v.z*v.z);
}

//! Returns a normalized (i.e. unit length) version of the vector passed in
inline Vector3 normalize(Vector3 v) {
	return v/length(v);
}


// --- Matrix operators ---

//! Multiply matrix and scalar, returns the new matrix
inline Matrix4 operator*(const Matrix4& m, const AReal& s) {
	Matrix4 m2 = Matrix4(m);
	m2.m[0] *= s;
	m2.m[1] *= s;
	m2.m[2] *= s;
	m2.m[3] *= s;
	m2.m[4] *= s;
	m2.m[5] *= s;
	m2.m[6] *= s;
	m2.m[7] *= s;
	m2.m[8] *= s;
	m2.m[9] *= s;
	m2.m[10] *= s;
	m2.m[11] *= s;
	m2.m[12] *= s;
	m2.m[13] *= s;
	m2.m[14] *= s;
	m2.m[15] *= s;
	return m2;
}

//! Multiply matrix and scalar, returns the new matrix (same as above, different order)
inline Matrix4 operator*(const AReal& s, const Matrix4& m) {
	return m*s;
}

//! Multiply matrix and point, returns the new point
//! Hint: Remember to homogenize the point!!!!!
inline Point3 operator*(const Matrix4& m, const Point3& p) {
	AReal scale = m.m[3]*p.x + m.m[7]*p.y+ m.m[11]*p.z + m.m[15];
	if (scale == 0)
		scale = EPSILON;
	return Point3((m.m[0]*p.x + m.m[4]*p.y + m.m[8]*p.z + m.m[12])/scale, 
		(m.m[1]*p.x + m.m[5]*p.y + m.m[9]*p.z + m.m[13])/scale, 
		(m.m[2]*p.x + m.m[6]*p.y + m.m[10]*p.z + m.m[14])/scale);
}

//! Multiply matrix and vector, returns the new vector
inline Vector3 operator*(const Matrix4& m, const Vector3& v) {
	return Vector3(m.m[0]*v.x + m.m[4]*v.y + m.m[8]*v.z,
		m.m[1]*v.x + m.m[5]*v.y + m.m[9]*v.z, 
		m.m[2]*v.x + m.m[6]*v.y + m.m[10]*v.z);
}

//! Multiply two matrices, returns the result
inline Matrix4 operator*(const Matrix4& m1, const Matrix4& m2) {
	Matrix4 mNew;
	for(int i = 0; i < 4; i++)
	{
		for(int j = 0; j < 4; j++)
		{
			mNew.m[i*4+j] = 0;
			for(int k = 0; k < 4; k++)
			{
				mNew.m[i*4+j] += m1.m[k*4+j]*m2.m[i*4+k];
			}
		}
	}
	return mNew;
}



// --- Constructors for Special Matrices ---

//! Returns the scale matrix described by the vector
inline Matrix4 scale_mat(const Vector3& v) {
	return Matrix4(v.x, 0, 0, 0,
		0,   v.y, 0, 0,
		0,   0,   v.z, 0,
		0,   0,   0,   1);
}

//! Returns the inverse scale matrix described by the vector
inline Matrix4 inv_scale_mat(const Vector3& v) {
	if (v.x != 0 && v.y != 0 && v.z != 0)
	{
		return Matrix4(1/v.x, 0, 0, 0,
			0,   1/v.y, 0, 0,
			0,   0,   1/v.z, 0,
			0,   0,   0,   1);
	}else{
		return Matrix4();
	}
}

//! Returns the translation matrix described by the vector
inline Matrix4 trans_mat(const Vector3& v) {
	return Matrix4(1, 0, 0, v.x,
		0, 1, 0, v.y,
		0, 0, 1, v.z, 
		0, 0, 0, 1);
}

//! Returns the inverse translation matrix described by the vector
inline Matrix4 inv_trans_mat(const Vector3& v) {
	return Matrix4(1, 0, 0, -v.x,
		0, 1, 0, -v.y,
		0, 0, 1, -v.z,
		0, 0, 0, 1);
}

//! Returns the rotation matrix about the x axis by the specified angle
inline Matrix4 rotX_mat(const AReal radians) {
	return Matrix4(1, 0, 0, 0,
		0, cos(radians), -sin(radians), 0,
		0, sin(radians), cos(radians),0,
		0, 0, 0, 1);
}

//! Returns the inverse rotation matrix about the x axis by the specified angle
inline Matrix4 inv_rotX_mat(const AReal radians) {
	return rotX_mat(-radians);
}

//! Returns the rotation matrix about the y axis by the specified angle
inline Matrix4 rotY_mat(const AReal radians) {
	return Matrix4(cos(radians), 0, sin(radians), 0,
		0, 1, 0, 0,
		-sin(radians), 0, cos(radians),0,
		0, 0, 0, 1);
}

//! Returns the inverse rotation matrix about the y axis by the specified angle
inline Matrix4 inv_rotY_mat(const AReal radians) {
	return rotY_mat(-radians);
}

//! Returns the rotation matrix about the z axis by the specified angle
inline Matrix4 rotZ_mat(const AReal radians) {
	return Matrix4(cos(radians), -sin(radians), 0, 0,
		sin(radians), cos(radians), 0, 0,
		0, 0, 1,0,
		0, 0, 0, 1);
}

//! Returns the inverse rotation matrix about the z axis by the specified angle
inline Matrix4 inv_rotZ_mat(const AReal radians) {
	return rotZ_mat(-radians);
}

//! Returns the rotation matrix around the vector v placed at point p, rotate by angle a
inline Matrix4 rot_mat(const Point3& p, const Vector3& v, const AReal a) {
	Vector3 translationVector = Vector3(p.x, p.y,p.z);
	//AReal yRotation = atan2(v.z, v.x);
	//AReal zRotation = -atan2(v.y, sqrt(v.x*v.x+v.z*v.z));
	AReal s = sin(a);
	AReal c = cos(a);
	AReal vt = 1-c;
	Vector3 k = normalize(v);
	Matrix4 rotation = Matrix4(k.x*k.x*vt+c, k.x*k.y*vt-k.z*s, k.x*k.z*vt +k.y*s, p.x,
		k.x*k.y*vt+k.z*s, k.y*k.y*vt+c, k.y*k.z*vt-k.x*s, p.y,
		k.x*k.z*vt-k.y*s, k.y*k.z*vt + k.x*s, k.z*k.z*vt+c, p.z,
		0,0,0,1);
	return rotation*inv_trans_mat(translationVector);
	//return trans_mat(translationVector)*inv_rotY_mat(yRotation)*inv_rotZ_mat(zRotation)*rotX_mat(a)
	//	*rotZ_mat(zRotation)*rotY_mat(yRotation)*inv_trans_mat(translationVector);
}

//! Returns the inverse rotation matrix around the vector and point by the specified angle
inline Matrix4 inv_rot_mat(const Point3& p, const Vector3& v, const AReal a) {
	return rot_mat(p, v, -a);
}



// --- Transpose, Inverse, and Other General Matrix Functions ---

//! Returns transpose of a matrix
inline Matrix4 transpose(const Matrix4& m) {
	return Matrix4(m.m[0], m.m[1], m.m[2], m.m[3],
		m.m[4], m.m[5], m.m[6], m.m[7],
		m.m[8], m.m[9], m.m[10], m.m[11],
		m.m[12], m.m[13], m.m[14], m.m[15]);
}

// Returns the determinant of the 3x3 matrix formed by excluding the specified row and column
// from the specified 4x4 matrix.  The formula for the determinant of a 3x3 is discussed on
// page 705 of Hill & Kelley, but note that there is a typo within the m_ij indices in the 
// equation in the book that corresponds to the cofactor02 line in the code below.
inline AReal subDeterminant(const Matrix4 &m, int excludeRow, int excludeCol) 
{
	// Compute non-excluded row and column indices
	int row[3];
	int col[3];

	int r=0;
	int c=0;
	for (int i=0; i<4; i++) {
		if (i != excludeRow) {
			row[r] = i;
			r++;
		}
		if (i != excludeCol) {
			col[c] = i;
			c++;
		}
	}

	// Compute the cofactors of each element in the first row
	AReal cofactor00 =    m(row[1],col[1]) * m(row[2],col[2])  -  m(row[1],col[2]) * m(row[2],col[1]);
	AReal cofactor01 = - (m(row[1],col[0]) * m(row[2],col[2])  -  m(row[1],col[2]) * m(row[2],col[0]));  
	AReal cofactor02 =    m(row[1],col[0]) * m(row[2],col[1])  -  m(row[1],col[1]) * m(row[2],col[0]);

	// The determinant is then the dot product of the first row and the cofactors of the first row
	return m(row[0],col[0])*cofactor00 + m(row[0],col[1])*cofactor01 + m(row[0],col[2])*cofactor02;
}

// Returns the cofactor matrix of M.  The cofactor matrix is a matrix where each element c_ij is the
// cofactor of the corresponding element m_ij in M.  The cofactor of each element m_ij is defined as
// (-1)^(i+j) times the determinant of the "submatrix" formed by deleting the i-th row and j-th 
// column from M.  See the definition in section A2.1.4 (page 705) in Hill & Kelley.   
inline Matrix4 cofactor(const Matrix4 &m) 
{
	Matrix4 out;
	// We'll use i to incrementally compute -1^(r+c)
	int i = 1;
	for (int r = 0; r < 4; ++r) {
		for (int c = 0; c < 4; ++c) {
			// Compute the determinant of the 3x3 submatrix
			AReal det = subDeterminant(m, r, c);
			out(r,c) = i * det;
			i = -i;
		}
		i = -i;
	}
	return out;
}

// Returns the determinant of the 4x4 matrix m
// See the hint in step 2 in Appendix A2.1.5 (page 706) in Hill & Kelley to learn how to compute this
inline AReal determinant(const Matrix4 &m)
{
	return m.m[0]*subDeterminant(m, 0, 0)-m.m[4]*subDeterminant(m, 0, 1)+m.m[8]*subDeterminant(m, 0, 2)-m.m[12]*subDeterminant(m, 0, 3);
}

// Returns the inverse of the 4x4 matrix m if m is nonsingular.  If m is singular, then returns the
// identity matrix. Hint: Look at the 4-step procedure outlined in Appendix A2.1.5 (page 706) in
// Hill & Kelley.
inline Matrix4 inverse(const Matrix4 &m)
{
	AReal determ = determinant(m);
	if (determ == 0)
	{
		return Matrix4();
	}else{
		Matrix4 cof = cofactor(m);
		cof = transpose(cof);
		cof = (1.0/determ)*cof;
		return cof;
	}
}


#endif


